import cv2 as cv
import numpy as np
import pickle
import json
import glob

objectpoints = np.zeros((6*7,3),np.float32)
objectpoints[:,:2] = np.mgrid[0:7,0:6].T.reshape(-1,2)

objectpoints_1 = []
imagepoints_1 = []

images = glob.glob('D:\photo\ew\*.jpg')
for frame in images:
    img = cv.imread(frame)
    gray = cv.cvtColor(img,cv.COLOR_BGR2GRAY)
    ret,corners = cv.findChessboardCorners(gray,(7,6),None)

    if ret == True:
        objectpoints_1.append(objectpoints)


        criteria = (cv.TERM_CRITERIA_MAX_ITER + cv.TERM_CRITERIA_EPS,50,0.001)
        corners_2 = cv.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
        imagepoints_1.append(corners_2)

        img = cv.drawChessboardCorners(img,(7,6),corners_2,ret)
        cv.imshow('img',img)
        cv.waitKey(2000)
#cv.destroyAllWindows()

#标定
ret,matrix,dist,rvecs,tvecs = cv.calibrateCamera(objectpoints_1,imagepoints_1,gray.shape,None,None)
print('ret\n',ret)
print('matrix\n',matrix)
print('dist\n',dist)
print('rvecs\n',rvecs)
print('tvecs\n',tvecs)

#储存
def save_json(json_file,file_name = 'jinjiezuoye_camera'):
    print(json_file)
    with open('./{}.json'.format(file_name),'wb') as f: #以二进制写方式打开，只能写文件， 如果文件不存在，创建该文件；如果文件已存在，先清空，再打开文件
        pickle.dump(json_file,f) #将json_file 保存到f
save_json({'ret':ret,'cameramat':matrix,'dist':dist,'rvecs':rvecs,'tvecs':tvecs})

with open('./jinjiezuoye_camera.json','rb') as f: #以二进制读方式打开，只能读文件 ， 如果文件不存在，会发生异常
    json_file = pickle.load(f)
    cameraMatrix = np.array(json_file['cameramat'], dtype=np.float)
    distCoeffs = np.array(json_file['dist'], dtype=np.float)
    objectpoints_1 = np.array(objectpoints_1,np.float64).reshape(11*6*7,3)
    imagepoints_1 = np.array(imagepoints_1,np.float64).reshape(11*6*7,2)
    objectpoints_1 = objectpoints_1[:9,:]
    imagepoints_1 = imagepoints_1[:9,:]
    retval,rvec,tvec = cv.solvePnP(objectpoints_1,imagepoints_1,cameraMatrix,distCoeffs)
    print("旋转变量", rvec)
    print("平移变量", tvec)